/*
 * 鏂囦欢鍚嶏細cpu0_config.c
 *
 * 缂栫爜鏍煎紡锛歎TF8
 *
 * 鍐呭绠�杩帮細PID鐩稿叧鍑芥暟
 *
 * 鍒涘缓浜�: 2025骞�1鏈�16鏃�
 * 淇敼璁板綍
 * 鏃ユ湡              浣滆��                澶囨敞
 * 2025-01-16       榛勭伅            first version
 */

#include "Algo_Pid.h"
#include "cpu0_config.h"


void Algo_BasicPID_Init(BasicPID_t *_tPid)
{
    if(_tPid == 0)
    {
        while(1);
    }

    _tPid->Error[0] = _tPid->Error[1] = _tPid->Pre_Out = _tPid->Ctrl_Out = 0.0f;

}


float Algo_BasicPID_Calculate(BasicPID_t *_tPid,float _fCurrValue,float _fExpValue,float dt)
{
    _tPid->Error[1] = _tPid->Error[0];
    _tPid->Error[0] = _fExpValue - _fCurrValue;

    _tPid->Pre_Out = _tPid->Ctrl_Out;
    _tPid->Ctrl_Out = _tPid->Kp * _tPid->Error[0] + _tPid->Kd * (_tPid->Error[0] - _tPid->Error[1]);

    if(_tPid->Ctrl_Out > _tPid->Max_Out) _tPid->Ctrl_Out = _tPid->Max_Out;
    if(_tPid->Ctrl_Out < -_tPid->Max_Out) _tPid->Ctrl_Out = -_tPid->Max_Out;

    return _tPid->Ctrl_Out;
}


void Algo_AdvancedPID_Init(AdvancedPID_t *_tPid)
{
    if(_tPid == 0)
    {
        while(1);
    }

    _tPid->Error[0] = _tPid->Error[1] = _tPid->Pre_Out = _tPid->Ctrl_Out =  _tPid->Iout = 0.0f;

}


float Algo_AdvancedPID_Calculate(AdvancedPID_t *_tPid,float _fCurrValue,float _fExpValue,float dt)
{
    _tPid->Error[1] = _tPid->Error[0];
    _tPid->Error[0] = _fExpValue - _fCurrValue;

    _tPid->Iout += _tPid->Ki * _tPid->Error[0] * dt;
    if(_tPid->Iout > _tPid->Max_Iout) _tPid->Iout = _tPid->Max_Iout;
    if(_tPid->Iout < -_tPid->Max_Iout) _tPid->Iout = -_tPid->Max_Iout;

    _tPid->Pre_Out = _tPid->Ctrl_Out;
    _tPid->Ctrl_Out = _tPid->Kp * _tPid->Error[0] + _tPid->Iout + _tPid->Kd * (_tPid->Error[0] - _tPid->Error[1]);

    if(_tPid->Ctrl_Out > _tPid->Max_Out) _tPid->Ctrl_Out = _tPid->Max_Out;
    if(_tPid->Ctrl_Out < -_tPid->Max_Out) _tPid->Ctrl_Out = -_tPid->Max_Out;

    return _tPid->Ctrl_Out;
}

void Algo_AdvancedPID_Clear(AdvancedPID_t *_tPid)
{
    if(_tPid == 0)
    {
        while(1);
    }

    _tPid->Error[0] = _tPid->Error[1] = _tPid->Pre_Out = _tPid->Ctrl_Out =  _tPid->Iout = 0.0f;

}


